
#include "Filt2.h"

Filt2::Filt2(float wn_,float am_, float Te_)
{
    if(Te_ <= 0.001)
    {
     Te_  = 0.001; 
    }
    
    if(2.0*M_PI/wn_ <= Te_)
    {
        wn_ = 2.0*M_PI/Te_;
    }
    
    wn = wn_;
    am = am_;
    Te = Te_;
    
    S = 0.0;
    E = 0.0; 
    S_m1 = 0.0;
    E_m1 = 0.0;
    S_m2 = 0.0;
    E_m2 = 0.0;
        
}

Filt2::~Filt2()
{
    
}

void Filt2::SetWn(float wn_)
{
	if(2.0*M_PI/wn_ <= Te)
    {
        wn_ = 2.0*M_PI/Te;
    }
    
    wn = wn_;
    
}

void Filt2::SetDamping(float am_)
{

    am = am_;
}

void Filt2::init(float init_s)
{
    E_m1 = init_s;
    E = init_s;
    S= init_s;
    S_m1 = init_s;
    S_m2 = init_s;
}

float Filt2::filt(float e_)
{
  
    
    float d2 = (4/(Te*Te*wn*wn)+4*am/(wn*Te)+1);
    float d1 = (2-8/(Te*Te*wn*wn));
    float d0 = (4/(Te*Te*wn*wn)-4*am/(wn*Te)+1);
	 
    E = e_;
    
    S = 1/d2*(E+2*E_m1+E_m2) - d1/d2*S_m1-d0/d2*S_m2;
    
    E_m1 =E;
    S_m1= S;
    E_m2 =E_m1;
    S_m2= S_m1;
    
    return S;
}


float Filt2::GetS()
{
	return S;
}
